// --------------------------------------------------------------------------------------------------------------------
// <copyright file="InverseKinematicsFilterMaximumClearanceFromPlane.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Kinematics
{
    using System;

    using Microsoft.Robotics.Numerics;

    using ShoNS.Array;

    /// <summary>
    /// A class that focuses on the clearance of each joint from a plane
    /// specified by the normal of the plane and a reference point on the
    /// plane.  This filter favors IK solutions where joints are further
    /// away from the specified plane.
    /// </summary>
    public class InverseKinematicsFilterMaximumClearanceFromPlane : InverseKinematicsFilter
    {
        /// <summary>
        /// The kinematic chain used to evaluate the joint clearance from boundary planes
        /// </summary>
        private KinematicChain kinematicChain;

        /// <summary>
        /// A list of booleans to indicate which joints we want to consider
        /// </summary>
        private bool[] jointCheckList;

        /// <summary>
        /// The normal of the plane
        /// </summary>
        private Vector3 normal;

        /// <summary>
        /// A reference point on the plane
        /// </summary>
        private Vector3 referencePoint;

        /// <summary>
        /// Initializes a new instance of the <see cref="InverseKinematicsFilterMaximumClearanceFromPlane" /> class.
        /// The plane is in the form of n * (p - x0) = 0.
        /// </summary>
        /// <param name="kc">The kinematic chain</param>
        /// <param name="checkList">A list of booleans to indicate which joints we want to consider</param>
        /// <param name="n">The normal of the plane</param>
        /// <param name="x0">A reference point on the plane</param>
        //// a link to the plane representation http://mathworld.wolfram.com/Plane.html
        public InverseKinematicsFilterMaximumClearanceFromPlane(KinematicChain kc, bool[] checkList, Vector3 n, Vector3 x0)
        {
            this.kinematicChain = kc;
            this.jointCheckList = checkList;
            this.normal = Vector3.Normalize(n);
            this.referencePoint = x0;
        }

        /// <summary>
        /// Compute the score of the joint configuration by
        /// examining the joint clearance from zero plane.
        /// </summary>
        /// <param name="jointConfiguration">A joint configuration</param>
        /// <returns>A score to evaluate the joint clearance</returns>
        public override double ScoreCalculator(DoubleArray jointConfiguration)
        {
            Matrix4[] transforms = this.kinematicChain.GetJointTransforms(jointConfiguration);

            double sum = 0;
            for (int i = 0; i < this.kinematicChain.JointList.Length; ++i)
            {
                if (this.jointCheckList[i])
                {
                    // compute the location of this joint
                    Vector3 jointLoc = new Vector3(transforms[i][0, 3], transforms[i][1, 3], transforms[i][2, 3]);

                    // collect the distance from this joint to the plane
                    double signedDistance = Vector3.Dot(this.normal, jointLoc) - Vector3.Dot(this.normal, this.referencePoint);

                    // if it is negative, the joint is behind the boundary plane
                    // a zero value will be returned which is the biggest non-positive
                    // value, which also indicates there is no clearance between the arm
                    // and the plane
                    if (signedDistance < 0)
                    {
                        return 0;
                    }
                    else
                    {
                        sum += signedDistance;
                    }
                }
            }

            return -sum;
        }
    }
}
